con

  _xinfreq = 5_000_000
  _clkmode = xtal1 + pll16x

  ENC1_A_PIN    = 0
  ENC1_B_PIN    = 1
  ENC2_A_PIN    = 2
  ENC2_B_PIN    = 3

var

  long  enc_count1, enc_count2  'must be contiguous

obj

  d     :       "Parallax Serial Terminal"
    
pub go

 result := cognew(@Entry,@enc_count1)

  d.start(115_200)

  repeat
    d.home
    d.str(string("Encoder1 Count: "))
    d.dec(enc_count1)
    d.chars(" ", 9)
    d.char(13)
    d.str(string("Encoder2 Count: "))
    d.dec(enc_count2)
    d.chars(" ", 9)
    d.char(13)
    waitcnt(cnt + clkfreq/100)
  
  
dat

              org

Entry
              'calc both encoder's pinmask
              mov       t0,a_pin1_mask
              or        t0,b_pin1_mask
              or        t0,a_pin2_mask
              or        t0,b_pin2_mask
              
              andn      dira,t0           'make encoder pins inputs

              'zero necessary vars
              mov       count1,#0
              mov       old_state1,#0
              mov       cur_state1,#0

              mov       count2,#0
              mov       old_state2,#0
              mov       cur_state2,#0

Main
              'sample quadrature encoder state
              test      a_pin1_mask,ina       wz
     if_nz    or        cur_state1,#1<<0        'set bit pos 0
              test      b_pin1_mask,ina       wz
     if_nz    or        cur_state1,#1<<1        'set bit pos 1
     
              test      a_pin2_mask,ina       wz
     if_nz    or        cur_state2,#1<<0        'set bit pos 0
              test      b_pin2_mask,ina       wz
     if_nz    or        cur_state2,#1<<1        'set bit pos 1

              'figure out the transition
              shl       old_state1,#2
              add       old_state1,cur_state1
              mov       t0,#table               'load table base addy
              add       t0,old_state1
              movs      :add_val1,t0

              shl       old_state2,#2
              add       old_state2,cur_state2
              mov       t0,#table               'load table base addy
              add       t0,old_state2
              movs      :add_val2,t0
              
              'remember the current state
              mov       old_state1,cur_state1
              mov       old_state2,cur_state2

              'zero the current state, gets ready for a new sample
              mov       cur_state1,#0
              mov       cur_state2,#0
              
              'add the appropriate value to the counter
:add_val1     adds      count1,0-0              'self modified code, the table entry's value gets put here
:add_val2     adds      count2,0-0

              'write the counter values up to hub mem
              wrlong    count1,par
              mov       t0,par
              add       t0,#4
              wrlong    count2,t0

              'do it again
              jmp       #Main
              
'================================================================================================
'             Initialized Variables
'================================================================================================

a_pin1_mask   long      1<<ENC1_A_PIN
b_pin1_mask   long      1<<ENC1_B_PIN
a_pin2_mask   long      1<<ENC2_A_PIN
b_pin2_mask   long      1<<ENC2_B_PIN

'quadrature encoder transition state change look up table
table         long      0,-1,1,0
              long      1,0,0,-1
              long      -1,0,0,1
              long      0,1,-1,0

'================================================================================================
'             Uninitialized Variables
'================================================================================================

count1        res       1
cur_state1    res       1
old_state1    res       1

count2        res       1
cur_state2    res       1
old_state2    res       1

t0            res       1